package source.realRobot.uC;

import source.calcCmd.RobotCommand;
import source.realRobot.Motor;
import source.realRobot.RealRobot;
import source.Enums.Flag;
import source.Enums.Side;
import source.Enums.SignalChange;


public class UC {

	private TimerCnt timerL;
	private TimerCnt timerR;
	
	private Port portL;
	private Port portR;
	
	private RealRobot robot;
	
	private int pathMod = 20;
	
	public UC(RealRobot robot, Motor MotorL, Motor MotorR){
		
		timerL = new TimerCnt();
		timerR = new TimerCnt();
		
		portL = new Port(MotorL);
		portR = new Port(MotorR);
		
		this.robot = robot;

	}
	
	public void runCmd(RobotCommand cmd){
		
		if (cmd.getStepsL() != 0){
			timerL.start(cmd.getStepsL(), cmd.getSpeedL());
		} else {
			robot.stopWheel(Side.LEFT);
		}
		if (cmd.getStepsR() != 0){
			timerR.start(cmd.getStepsR(), cmd.getSpeedR());
		} else {
			robot.stopWheel(Side.RIGHT);
		}
		
		long prevTick = 0;
		long elapsed = 0;

		while (timerL.getReady() == Flag.BUSY || timerR.getReady() == Flag.BUSY){
			
			long nextLeft = timerL.getLastTick() + timerL.getNextTickIn();
			long nextRight = timerR.getLastTick() + timerR.getNextTickIn();

			if (nextLeft >= nextRight && timerR.getReady() == Flag.BUSY || (timerL.getReady() == Flag.READY && timerR.getReady()==Flag.BUSY)){
							
				elapsed = nextRight - prevTick;
				prevTick = nextRight;
				robot.move(elapsed);
				
				timerR.setLastTick(nextRight);
				portR.invert(nextRight);
				if (portR.getChange() == SignalChange.DESCEND){
					timerR.tick();
				}
				
				if (timerR.getReady() == Flag.READY && cmd.getSpeedR() == TimerCnt.maxValue){
					robot.stopWheel(Side.RIGHT);
				}
				
			} else if (timerL.getReady() == Flag.BUSY){
				
				elapsed = nextLeft - prevTick;
				prevTick = nextLeft;
				robot.move(elapsed);
				
				timerL.setLastTick(nextLeft);
				portL.invert(nextLeft);
				if (portL.getChange() == SignalChange.DESCEND){
					timerL.tick();
				}
				
				if (timerL.getReady() == Flag.READY && cmd.getSpeedR() == TimerCnt.maxValue){
					robot.stopWheel(Side.LEFT);
				}
				
			}

		}
		
	}
	
	public void setTimersNextSpeed(int nextSpeedL, int nextSpeedR){
		if (nextSpeedL == 0){
			nextSpeedL = TimerCnt.maxValue;
		}
		if (nextSpeedR == 0){
			nextSpeedR = TimerCnt.maxValue;
		}
		timerL.setNextSpeed(nextSpeedL);
		timerR.setNextSpeed(nextSpeedR);
	}
	
	public void setTimers(int speedL, int speedR){
		if (speedL == 0){
			speedL = TimerCnt.maxValue;
		}
		if (speedR == 0){
			speedR = TimerCnt.maxValue;
		}
		timerL.setCurrentSpeed(speedL);
		timerR.setCurrentSpeed(speedR);
	}
	
	public void reset() {
		timerL.reset();
		timerR.reset();
		
		portL.reset();
		portR.reset();
		
	}
	
	public int getSpeed(Side which){
		int sp = 0;
		
		switch(which){
		case RIGHT:
			sp = timerR.getSpeed();
			break;
		case LEFT:
			sp = timerL.getSpeed();
			break;
		}
		
		return sp;
	}

	
}
